#include "CatmullRomCurveEvaluator.h"
#include <cassert>

void CatmullRomCurveEvaluator::evaluateCurve(const std::vector<Point>& ptvCtrlPts, 
                                          std::vector<Point>& ptvEvaluatedCurvePts, 
                                          const float& fAniLength, 
                                          const bool& bWrap) const
{
    int iCtrlPtCount = ptvCtrlPts.size();

    //At least 4 Control points are needed
    if(iCtrlPtCount >= 2) {

        int stepping = 75;
        ptvEvaluatedCurvePts.clear();

        //Creating "Phantom" Points at the beginning:
        std::vector<Point> ptvCtrlPtsB;
        if(bWrap) {
            ptvCtrlPtsB.push_back(Point(-fAniLength+ptvCtrlPts[iCtrlPtCount-2].x, ptvCtrlPts[iCtrlPtCount-2].y));
            ptvCtrlPtsB.push_back(Point(-fAniLength+ptvCtrlPts[iCtrlPtCount-1].x, ptvCtrlPts[iCtrlPtCount-1].y));
        } else {
            ptvCtrlPtsB.push_back(Point(-2.0, ptvCtrlPts[0].y));
            ptvCtrlPtsB.push_back(Point(-2.0, ptvCtrlPts[0].y));
        }
        
        ptvCtrlPtsB.insert(ptvCtrlPtsB.end(), ptvCtrlPts.begin(), ptvCtrlPts.end() );
        //Creating "Phantom" Points at the end:
        if(bWrap) {
            ptvCtrlPtsB.push_back(Point(ptvCtrlPts[0].x + fAniLength, ptvCtrlPts[0].y));
            ptvCtrlPtsB.push_back(Point(ptvCtrlPts[1].x + fAniLength, ptvCtrlPts[1].y));
        } else { 
            ptvCtrlPtsB.push_back(Point(fAniLength+2, ptvCtrlPts[iCtrlPtCount-1].y));
            ptvCtrlPtsB.push_back(Point(fAniLength+2, ptvCtrlPts[iCtrlPtCount-1].y));
        }
              
        int iCtrlPtCountB = ptvCtrlPtsB.size();

        double a[4];
        double b[4];

        for(int i = 1; i < (iCtrlPtCountB-2); i++) {         
                    
            a[0] = (-ptvCtrlPtsB[i-1].x + 3*ptvCtrlPtsB[i].x - 3*ptvCtrlPtsB[i+1].x + ptvCtrlPtsB[i+2].x)/2.0;
            a[1] = (2*ptvCtrlPtsB[i-1].x - 5*ptvCtrlPtsB[i].x + 4*ptvCtrlPtsB[i+1].x - ptvCtrlPtsB[i+2].x)/2.0;
            a[2] = (-ptvCtrlPtsB[i-1].x + ptvCtrlPtsB[i+1].x)/2.0;
            a[3] = (2*ptvCtrlPtsB[i].x)/2.0;

            b[0] = (-ptvCtrlPtsB[i-1].y + 3*ptvCtrlPtsB[i].y - 3*ptvCtrlPtsB[i+1].y + ptvCtrlPtsB[i+2].y)/2.0;
            b[1] = (2*ptvCtrlPtsB[i-1].y - 5*ptvCtrlPtsB[i].y + 4*ptvCtrlPtsB[i+1].y - ptvCtrlPtsB[i+2].y)/2.0;
            b[2] = (-ptvCtrlPtsB[i-1].y + ptvCtrlPtsB[i+1].y)/2.0;
            b[3] = (2*ptvCtrlPtsB[i].y)/2.0;

            for(int i = 0; i < stepping ; i++) {
                float t;
                t = ((float)i)/((float)stepping); 
                ptvEvaluatedCurvePts.push_back(Point( a[3] + t*(a[2] + t*(a[1] + t*a[0])),
                    b[3] + t*(b[2] + t*(b[1] + t*b[0])) ));
            }
        }       
    }
}